package orbito.physics.motion;

import java.util.LinkedList;
import java.util.List;

import orbito.physics.motion.State.Derivative;

public class EulerIntegrator implements Integrator {
	
	List<Actor> actors;
	ForceManager forceManager;
	
	public EulerIntegrator(ForceManager forceManager) {
		this.actors = new LinkedList<Actor>();
		this.forceManager = forceManager;
	}
	
	public List<Actor> getActors() {
		return actors;
	}
	
	public void addActor(Actor actor) {
		actors.add(actor);
	}
	
	public void removeActor(Actor actor) {
		actors.remove(actor);
	}
	
	public ForceManager getForceManager() {
		return forceManager;
	}
	
	public void setForceManager(ForceManager forceManager) {
		this.forceManager = forceManager;
	}
	
	public void integrate(double t, double dt) {
		// Calculate forces once, at time t.
		forceManager.calculateForces(t);
		
		// Evaluate derivatives at time t, and calculate new states at time t + dt, using derivatives.
		for (Actor actor : actors) {
			Derivative d = evaluate(actor);
			actor.state = calculate(actor.state, dt, d);
		}
	}
	
	private Derivative evaluate(Actor actor) {
		return new Derivative(
				actor.state.v,
				actor.getAcceleration(),
				actor.state.w,
				actor.getAngularAcceleration());
	}
	
	private State calculate(State initial, double dt, Derivative d) {
		return new State(
				initial.x.add(d.dx.mul(dt)),
				initial.v.add(d.dv.mul(dt)),
				initial.a + d.da * dt,
				initial.w + d.dw * dt);
	}
}
